﻿Imports System.Text
Imports System.Threading
Imports System.Text.Json
Public Class RobotPort : Inherits WebPort

    Public Const Api As String = "/api/"
    Public Const ApiMove As String = "/api/move/"
    Public Const ApiMarkers As String = "/api/markers/"
    Public Const MaxLinear As Double = 0.2 '[m/s]
    Public Const MaxAngular As Double = 0.5
    Public ParseDict As Dictionary(Of String, Action(Of String)) = New Dictionary(Of String, Action(Of String)) From {
        {"/api/markers/query_brief", AddressOf ParseMarkersInfo},
        {"/api/robot_status", AddressOf ParseRobotStatus},
        {"/api/robot_info", AddressOf ParseRobotInfo}
        }
    Public Event OnGetMarkerList As Action(Of String()) '刷新标记点后的事件
    Public Event OnGetRobotStatus As Action
    Public ReadOnly Property CmdPort As Integer
        Get
            Return 31001
        End Get
    End Property

    Private Receiving As Boolean = False
    Public ReadOnly Property IsReceiving As Boolean
        Get
            Return Receiving
        End Get
    End Property
    Public Property MarkerLists As String()
    Public Property Status As New WaterMessage.robot_status.ResultsType
    Private RxMsgQueue As Queue(Of String) = New Queue(Of String)()

    Public Sub StopReceiving()
        Receiving = False
    End Sub

    Public Sub GetRobotInfo()
        Write(Api + "robot_info")
    End Sub

    Public Sub CancelMove()
        Write(ApiMove + "cancel")
    End Sub

    Public Sub GetRobotStatus()
        Write(Api + "robot_status")
    End Sub

    Public Sub GetMarkersInfo()
        Write(ApiMarkers + "query_brief")
    End Sub

    Public Sub MoveToMarker(marker_name As String)
        Write($"/api/move?marker={marker_name}")
    End Sub

    Public Sub SetVelocity(linear As Double, angular As Double)
        Write($"/api/joy_control?angular_velocity={angular.ToString("F1")}&linear_velocity={linear.ToString("F1")}")
    End Sub

    Public Function Parse(msg As String) As Boolean
        Try
            Dim msgjson = JsonSerializer.Deserialize(Of WaterMessage.Base)(msg)
            If Not ParseDict.ContainsKey(msgjson.command) Then
                Return False
            End If
            ParseDict(msgjson.command)(msg)
            Return True
        Catch ex As Exception
            Console.WriteLine(ex.Message)
            Return False
        End Try
    End Function

    Private Function ParseMarkersInfo(msg As String) As Boolean
        Dim marker_brief = JsonSerializer.Deserialize(Of WaterMessage.query_brief)(msg)
        If marker_brief.status <> "OK" Then
            Return False
        End If
        MarkerLists = marker_brief.results.Keys.ToArray()
        Console.WriteLine($"Available Markers: {MarkerLists.Length}")
        Try
            RaiseEvent OnGetMarkerList(MarkerLists)
        Catch ex As Exception
            Console.WriteLine(ex.Message)
            Return False
        End Try
        Return True
    End Function

    Private Function ParseRobotStatus(msg As String) As Boolean
        Dim robot_state = JsonSerializer.Deserialize(Of WaterMessage.robot_status)(msg)
        If robot_state.status <> "OK" Then
            Return False
        End If
        Status = robot_state.results
        RaiseEvent OnGetRobotStatus()
        Return True
    End Function

    Private Sub ParseRobotInfo(msg As String)
        Dim robot_info = JsonSerializer.Deserialize(Of WaterMessage.robot_info)(msg)
        MsgBox($"Product Id: {robot_info.results.product_id}", MsgBoxStyle.Information)
    End Sub

    Public Sub StartReceiving()

        If Receiving Then
            Console.WriteLine("Already Receving.")
            Return
        End If

        Task.Run(Sub()
                     Receiving = True
                     Dim HistoryBytes = 0
                     Console.WriteLine("Robot Start Receiving.")
                     While True
                         If Not Receiving Then Exit While
                         If TcpComm.Client Is Nothing Then
                             Continue While
                         End If
                         If HistoryBytes = BytesToRead And BytesToRead <> 0 Then
                             Dim buf(BytesToRead - 1) As Byte
                             TcpComm.Client.Receive(buf)
                             Dim msg = Encoding.ASCII.GetString(buf)
                             Dim msg_list = msg.Split(vbCrLf)
                             For Each m As String In msg_list
                                 Console.WriteLine(m)
                                 RxMsgQueue.Enqueue(m)
                             Next
                         End If
                         HistoryBytes = BytesToRead
                         Thread.Sleep(5)
                     End While
                     Receiving = False
                     Console.WriteLine("Robot Stop Receving Message.")
                 End Sub)

        Task.Run(Sub()
                     Console.WriteLine("Robot Start Parsing Message.")
                     While True
                         If Not Receiving Then Exit While
                         If RxMsgQueue.Count > 0 Then
                             Dim webmsg = RxMsgQueue.Dequeue()
                             Parse(webmsg)
                         End If
                         Thread.Sleep(5)
                     End While
                     Console.WriteLine("Robot Stop Parsing Message.")
                 End Sub)

    End Sub

End Class
